import math
import os
import sys
import importlib
import json
import time
from scipy.spatial import cKDTree
import numpy as np
# 第三方库
import numpy as np

kdtree = importlib.import_module("onsite-unstructured.common.kdtree")

class Item(object):
    def __init__(self, x, y, data):
        self.coords = (x, y)
        self.data = data

    def __len__(self):
        return len(self.coords)

    def __getitem__(self, i):
        return self.coords[i]

    def __repr__(self):
        return "Item({},{},{})".format(self.coords[0], self.coords[1], self.data)
def creat_kdtree(path_waypoints):
    """创建k-d树

    Args:
        path_waypoints (list): 规划路径点
    """
    path_tuple = list()
    i = 0  # 记录路径点下标
    for waypoint in path_waypoints:
        point = Item(waypoint[0], waypoint[1], str(i))  # ! (x,y,index) 用于构建KD-tree
        path_tuple.append(point)
        i += 1
    tree = kdtree.create(path_tuple)
    return tree

def lqr_tree(c_path):
    ""
    """在路径规划阶段调用一次，构建 KD-Tree。"""
    cx_np = np.array(c_path[0])
    cy_np = np.array(c_path[1])
    points = np.vstack((cx_np, cy_np)).T  # shape (N,2)
    lqr_kdtree = cKDTree(points)

    return lqr_kdtree